close all;clear all;clc;
%一：载入图片
imgcount = 12;%需要处理的图片数量
imageFileNames = {1,imgcount};
for i = 1:imgcount
  imageFileNames{i} = sprintf('images/%d.jpg', i);
end
%二：图像处理
%%imagePoints是像素坐标系下的x,y坐标值
[imagePoints, boardSize, imagesUsed] = detectCheckerboardPoints(imageFileNames);%标记角点
imageFileNames = imageFileNames(imagesUsed);
originalImage = imread(imageFileNames{1});
imageSize = size(originalImage);%记录图像的长M和宽N
imageSize = imageSize(:,1:2);
%三：从世界坐标系转换到相机坐标系
%%建立世界坐标系
% squareSize = 25;
squareSize = 0.031;
boardSize = double(boardSize)-1;%为什么这里要减一？
worldPoints = zeros(boardSize(1) * boardSize(2), 2);%建立一个6*7=42个点的二维坐标
k = 1;
for j = 0:boardSize(2)-1
    for i = 0:boardSize(1)-1
        worldPoints(k,1) = j * squareSize;
        worldPoints(k,2) = i * squareSize;
        k = k + 1;
    end
end
%四：计算单应矩阵
params = estimateCameraParameters(imagePoints, worldPoints, 'EstimateTangentialDistortion', true, ...
    'NumRadialDistortionCoefficients',2,'ImageSize', imageSize);
numImages = numel(imageFileNames);%实际可用图片的个数
homographies = zeros(3, 3, numImages); %建立一个3*3的矩阵
H=zeros(3,3,4);
for i = 1:numImages
        homographies(:, :, i) = ...
            compute_single_H(double(imagePoints(:,:,i)),worldPoints)';
       % H(:,:,i) = homographies(:,:,i);
end
%五：计算张正波大牛的v矩阵
V = zeros(2 * numImages, 6); %因为V是6个方程
for i = 1:numImages
    H = homographies(:, :, i)';   %为什么这里要加转置
    V(i*2-1,:) = computeLittleV(H, 1, 2);
    V(i*2, :) = computeLittleV(H, 1, 1) - computeLittleV(H, 2, 2);
end
[~, ~, v] = svd(V); %最小奇异值求解B 这里的奇异值求解复习高工数
b=v(:,end);%最小奇异值
B = [b(1), b(2), b(4); b(2), b(3), b(5); b(4), b(5), b(6)];%这里是对称矩阵
%六：计算内参
v0 = (B(1,2)*B(1,3)-B(1,1)*B(2,3))/(B(1,1)*B(2,2)-B(1,2)*B(1,2));
lamda = B(3,3)-(B(1,3)^2+v0*(B(1,2)*B(1,3)-B(1,1)*B(2,3)))/B(1,1);
fu=(lamda/B(1,1))^(1/2);
fv=(lamda*B(1,1)/(B(1,1)*B(2,2)-B(1,2)^2))^(1/2);
c=-B(1,2)*fu^2*fv/lamda;
u0=c*v0/fv-B(1,3)*fu^2/lamda;
A = [fu,c,u0;0,fv,v0;0,0,1]; %这里转置是为了与那个工具箱标定形式相同
%七：计算每幅图像的外参
[rotation, translation] = computeExtrinsics(A, homographies);
%八：计算切向畸变参数
% initial guess for radial distortion
D=[];
d=[];
Rm=[];
m_UV = [];
m_XY = [];
Ainv = inv(A);%A的逆
matrixone  = ones(length(worldPoints),1);
M = [worldPoints,matrixone]';
npts = length(imagePoints(:,1,1));%角点的数目
for count = 1:1:numImages
    %估计尺度因子，参考3.1节
    m_H = homographies(:,:,count);
    s = 1/norm(Ainv*m_H);
    m_R = rotationVectorToMatrix(rotation(count,:));%1*3
    m_T = translation(count,:);%1*3
    RT = [m_R(:,1),m_R(:,2),m_T'];
    XY = RT*M;
    UV = A*XY;
    UV=[UV(1,:)./UV(3,:); UV(2,:)./UV(3,:); UV(3,:)./UV(3,:)];
    XY=[XY(1,:)./XY(3,:); XY(2,:)./XY(3,:); XY(3,:)./XY(3,:)];%3*1的矩阵
    m_imagePoint = imagePoints(:,:,count)';
    for j =1:1:npts
            D=[D; ((UV(1,j)-u0)*( (XY(1,j))^2 + (XY(2,j))^2 )) , ((UV(1,j)-u0)*( (XY(1,j))^2 + (XY(2,j))^2 )^2) ; ((UV(2,j)-v0)*( (XY(1,j))^2 + (XY(2,j))^2 )) , ((UV(2,j)-v0)*( (XY(1,j))^2 + (XY(2,j))^2 )^2) ];
            d=[d; (m_imagePoint(1,j)-UV(1,j)) ; (m_imagePoint(2,j)-UV(2,j))];
    end
end
% 估计k1,k2 参考公式13
m_k=(D'*D)\D'*d;
%九：极大似然估计优化参数，使用LM算法
% 定义初始相机参数
%A = [fu,c,u0;0,fv,v0;0,0,1]; %这里转置是为了与那个工具箱标定形式相同
%K0 = A'; % 初始内参矩阵 
R0 = rotation(1,:); % 初始旋转矩阵 1*3的矩阵
t0 = translation(1,:);%初始平移矩阵 1*3的矩阵
x0 = [fu;c; u0;fv;v0;R0(:); t0(:);m_k]; % 初始相机参数 K0(:)这是按一列一列进行排序的5+3+3+2
% 定义误差函数
fun = @(x) calibration_error(x, imagePoints, worldPoints);
% 定义LM算法参数
options = optimoptions(@lsqnonlin, 'Algorithm', 'levenberg-marquardt', 'Display', 'off');
% 使用LM算法求解最小化误差函数
x = lsqnonlin(fun, x0, [], [], options);
%x = LMFsolve(fun,x0,options);
% 输出估计的相机参数
K_new = [x(1),x(2),x(3);0,x(4),x(5);0,0,1];
R_new = x(6:8);%旋转向量
t_new = x(9:11);%平移向量
m_k_new = x(12:13);%畸变系数1*2的矩阵
%pix_cor = get_single_project_coor(K_new',R_new',t_new',worldPoints(42,:))
%%
%计算V
function v = computeLittleV(H, i, j)
        v = [H(i,1)*H(j,1), H(i,1)*H(j,2)+H(i,2)*H(j,1), H(i,2)*H(j,2),...
         H(i,3)*H(j,1)+H(i,1)*H(j,3), H(i,3)*H(j,2)+H(i,2)*H(j,3), H(i,3)*H(j,3)];
end
%计算B

%计算单应矩阵
function H = compute_single_H(imagePoints, worldPoints)

    %minRequiredNonCollinearPairs = 4;%最少4个角点
    xy = imagePoints;%像素坐标系坐标
    uv = worldPoints;%世界坐标系坐标

    M=size(xy,1);
    x = xy(:,1);  %像素坐标系下x坐标
    y = xy(:,2);  %像素坐标系下y坐标
    vec_1 = ones(M,1);
    vec_0 = zeros(M,1);
    u = uv(:,1); %世界坐标系下u坐标
    v = uv(:,2); %世界坐标系下v坐标
    U = [u; v];
    X = [x y vec_1 vec_0 vec_0 vec_0 -x.*u -y.*u ;...
        vec_0 vec_0 vec_0 x y vec_1 -x.*v -y.*v ];
    % u = (Ax + By + C)/(Gx + Hy + I)
    % v = (Dx + Ey + F)/(Gx + Hy + I)
    Tvec = X \ U;   %这个公式原理在上面注释
    Tvec(9) = 1; %因为第九个自由度是约束的
    Tinv = reshape(Tvec,3,3);
    T = inv(Tinv);      %求逆，因为上面的是X=HU，其实应该是U=HX，因为像素坐标系是在世界坐标系之后
    T = T ./ T(3,3);
    H = T;
end
%%计算外参
function [rotationVectors, translationVectors] = ...
    computeExtrinsics(A, homographies)
% Compute translation and rotation vectors for all images
numImages = size(homographies, 3);
rotationVectors = zeros(3, numImages);
translationVectors = zeros(3, numImages); 
Ainv = inv(A);
for i = 1:numImages
    H = homographies(:, :, i);
    h1 = H(:, 1);
    h2 = H(:, 2);
    h3 = H(:, 3);
    lambda = 1 / norm(Ainv * h1); %#ok
    
    % 3D rotation matrix
    r1 = lambda * Ainv * h1; %#ok
    r2 = lambda * Ainv * h2; %#ok
    r3 = cross(r1, r2);
    R = [r1,r2,r3];
    
    rotationVectors(:, i) = vision.internal.calibration.rodriguesMatrixToVector(R);
    
    % translation vector
    t = lambda * Ainv * h3;  %#ok
    translationVectors(:, i) = t;
end

rotationVectors = rotationVectors';
translationVectors = translationVectors';
end

function [pix_cor] = get_single_project_coor(A,R,T,K, WorldPoint_Val)
%A是相机内参
%R:旋转矩阵的r1和r2
%T;平移矩阵
%WorldPoint_Val空间坐标
%pix_cor返回图形坐标
%example:pix_cor = get_single_project_coor(homographies(:,:,1),rotation(1,:),translation(1,:),worldPoints(1,:))
%%
X = WorldPoint_Val(1,1);
Y = WorldPoint_Val(1,2);
M = [X;Y;1];
k1 = K(:,1);
k2 = K(:,2);
%1.定义H投影矩阵
R_val = rotationVectorToMatrix(R);
RT_C_1 = [R_val(:,1),R_val(:,2),T'];
XY = RT_C_1*M;
UV = A*XY;
UV=[UV(1,:)./UV(3,:); UV(2,:)./UV(3,:); UV(3,:)./UV(3,:)];
XY=[XY(1,:)./XY(3,:); XY(2,:)./XY(3,:); XY(3,:)./XY(3,:)];%3*1的矩阵
R2 = XY(1,:)^2 + XY(2,:)^2;
D(1,:) = UV(1,:) + (UV(1,:)-A(1,3))*(k1*R2+k2*(R2^2));
D(2,:) = UV(2,:) + (UV(2,:)-A(2,3))*(k1*R2+k2*(R2^2));
% 输出像素坐标
pix_cor = D;
%定义3*4的H投影矩阵
%2.因为有很多张图片，因此需要先了解到每张图片有多少个角点被识别到，另外每张图片对应一个外参
%t：平移矩阵是1*3个参数 r：旋转矩阵也是1*3个参数因为没做变换，所以每张图片外参有6个，内参5个
%imagePoints:识别到的角点坐标数目，worldPoint是角点的空间坐标
%我们分别将参数都设为当前的参数作为初始值，然后放入LM算法中进行迭代
%其迭代的函数是
end

function [error] = calibration_error(x, check_points, WorldCheck_Points)
%x是矩阵参数包括外参和内参
%check_points棋盘格像素坐标系坐标
%WorldCheck_Points棋盘格对应的空间坐标系的坐标
%return error反投影误差
% 解析优化变量
% K = reshape(x(1:9), 3, 3)';%内参
K = [x(1),x(2),x(3);0,x(4),x(5);0,0,1];
R = x(6:8)';%旋转向量
t = x(9:11)';%平移向量
k = x(12:13)';%畸变系数1*2的矩阵
% 初始化误差向量
error = [];
m = size(check_points(:,:,:));
% 遍历所有棋盘格图像
for j = 1:1:m(3)
    for i = 1:1:length(check_points(:,:,j))%for i = 1:1:length(point) point = imagePoints(:,:,1)
    % 读取图像和对应的图像坐标
        point_observed = check_points(i,:,j);%point_observed = point(i,:);
    % 计算估计的图像坐标
        point_estimated = get_single_project_coor(K,R,t,k, WorldCheck_Points(i,:))';
    % 计算重投影误差
        e = point_observed-point_estimated;
    % 将误差向量添加到总误差中
        error = [error; e(:)];
    end
end
end

function R = vec2rpy(rpy)
% 将欧拉角转换为旋转矩阵
% rpy: 欧拉角

roll = rpy(1);
pitch = rpy(2);
yaw = rpy(3);

Rx = [1, 0, 0; 0, cos(roll), -sin(roll); 0, sin(roll), cos(roll)];
Ry = [cos(pitch), 0, sin(pitch); 0, 1, 0; -sin(pitch), 0, cos(pitch)];
Rz = [cos(yaw), -sin(yaw), 0; sin(yaw), cos(yaw), 0; 0, 0, 1];

R = Rz * Ry * Rx;

end